import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches


# ================================================== 工具类 ===============================================
# 读取 .csv 中的经纬度字段 (字段名: key_lon, key_lat)
def read_lonlat_from_csv(path_csv, key_lon, key_lat):
    df = pd.read_csv(path_csv)
    return np.array(df[key_lon]), np.array(df[key_lat])


# 使用Haversine公式计算两点间距离（以米为单位）
def haversine_distance(lat1, lon1, lat2, lon2):
    R = 6371000  # 地球半径，单位米
    lat1, lon1, lat2, lon2 = map(np.radians, [lat1, lon1, lat2, lon2])
    dlat = lat2 - lat1
    dlon = lon2 - lon1
    a = np.sin(dlat/2)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(dlon/2)**2
    c = 2 * np.arcsin(np.sqrt(a))
    distance = R * c
    return distance


# ================================================== 画图 ===============================================
# 绘制单一轨迹
def plot_trace_single(lon_real, lat_real, lon_aeconvlstm, lat_aeconvlstm):
    plt.figure(figsize=(12, 8))
    # 绘制真实轨迹
    plt.plot(lon_real, lat_real, color='black', linewidth=2, label='Real', zorder=5)
    # 绘制算法的轨迹
    plt.plot(lon_aeconvlstm, lat_aeconvlstm, color='blue', linestyle='--', linewidth=1.5, label='Proposed')
    # 起点和终点标记
    plt.scatter(lon_real[0], lat_real[0], color='black', marker='o', s=100, zorder=6)
    plt.scatter(lon_real[-1], lat_real[-1], color='black', marker='s', s=100, zorder=6)
    plt.text(lon_real[0] + 0.0001, lat_real[0] + 0.0001, 'Start', fontsize=12, color='black')
    plt.text(lon_real[-1] + 0.0001, lat_real[-1] + 0.0001, 'End', fontsize=12, color='black')
    #
    plt.xlabel("Longitude (°)", fontsize=12)
    plt.ylabel("Latitude (°)", fontsize=12)
    # plt.title("Trajectory Comparison: Real vs. AE-Conv-LSTM, CAEKF, IMMUKF", fontsize=14)
    plt.legend(fontsize=12)
    plt.grid(True, alpha=0.5)
    plt.tight_layout()
    # plt.savefig("trajectory_comparison.png", dpi=300)
    plt.show()
    # print("轨迹对比图已保存为 'trajectory_comparison.png'")

# 绘制所有轨迹
def plot_trace(lon_real, lat_real, lon_aeconvlstm, lat_aeconvlstm, lon_caekf, lat_caekf, lon_immukf, lat_immukf):
    plt.figure(figsize=(12, 8))
    # 绘制真实轨迹
    plt.plot(lon_real, lat_real, color='black', linewidth=2, label='Real Trajectory', zorder=5)
    # 绘制三种算法的轨迹
    plt.plot(lon_aeconvlstm, lat_aeconvlstm, color='blue', linestyle='--', linewidth=1.5, label='Proposed')
    plt.plot(lon_caekf, lat_caekf, color='green', linestyle='-.', linewidth=1.5, label='CAEKF')
    plt.plot(lon_immukf, lat_immukf, color='red', linestyle=':', linewidth=1.5, label='IMMUKF')
    # 起点和终点标记
    plt.scatter(lon_real[0], lat_real[0], color='black', marker='o', s=100, zorder=6)
    plt.scatter(lon_real[-1], lat_real[-1], color='black', marker='s', s=100, zorder=6)
    plt.text(lon_real[0] + 0.0001, lat_real[0] + 0.0001, 'Start', fontsize=12, color='black')
    plt.text(lon_real[-1] + 0.0001, lat_real[-1] + 0.0001, 'End', fontsize=12, color='black')
    #
    plt.xlabel("Longitude (°)", fontsize=12)
    plt.ylabel("Latitude (°)", fontsize=12)
    plt.title("Trajectory Comparison: Real vs. AE-Conv-LSTM, CAEKF, IMMUKF", fontsize=14)
    plt.legend(fontsize=12)
    plt.grid(True, alpha=0.5)
    plt.tight_layout()
    # plt.savefig("trajectory_comparison.png", dpi=300)
    plt.show()
    # print("轨迹对比图已保存为 'trajectory_comparison.png'")


# 图1：位置误差时间序列对比图（折线图） - 支持对数坐标
def plot_err_linechart_log(timesteps, error_aeconvlstm, error_caekf, error_immukf):
    # 处理可能的零值，因为 log(0) 未定义
    eps = 1e-10
    error_aeconvlstm_log = np.log10(error_aeconvlstm + eps)
    error_caekf_log = np.log10(error_caekf + eps)
    error_immukf_log = np.log10(error_immukf + eps)
    plt.figure(figsize=(12, 6))
    plt.plot(timesteps, error_aeconvlstm_log, color='blue', linewidth=1.2, label='Proposed (log10)')
    plt.plot(timesteps, error_caekf_log, color='green', linewidth=1.2, label='CAEKF (log10)')
    plt.plot(timesteps, error_immukf_log, color='red', linewidth=1.2, label='IMMUKF (log10)')
    plt.xlabel("Time Step", fontsize=12)
    plt.ylabel("Log10(Position Error) (log10(m))", fontsize=12)
    # plt.title("Position Error Over Time (Log Scale)", fontsize=14)
    plt.legend(fontsize=12)
    plt.grid(True, alpha=0.5)
    plt.tight_layout()
    plt.show()


# 图1：位置误差时间序列对比图（折线图）
def plot_err_linechart(timesteps, error_aeconvlstm, error_caekf, error_immukf):
    plt.figure(figsize=(12, 6))
    plt.plot(timesteps, error_aeconvlstm, color='blue', linewidth=1.2, label='Proposed')
    plt.plot(timesteps, error_caekf, color='green', linewidth=1.2, label='CAEKF')
    plt.plot(timesteps, error_immukf, color='red', linewidth=1.2, label='IMMUKF')
    #
    plt.xlabel("Time Step", fontsize=12)
    plt.ylabel("Position Error (m)", fontsize=12)
    # plt.title("Position Error Over Time", fontsize=14)
    plt.legend(fontsize=12)
    plt.grid(True, alpha=0.5)
    plt.tight_layout()
    # plt.savefig("position_error_time_series.png", dpi=300)
    plt.show()
    # print("位置误差时间序列图已保存为 'position_error_time_series.png'")


# 图2：位置误差分布直方图（分组柱状图）
def plot_err_barchart(error_aeconvlstm, error_caekf, error_immukf):
    # 定义误差区间（例如每50米一个区间）
    # bins = [0, 50, 200, 1000, 5000, 10000, 30000, 50000]
    # bin_labels = ['0-50', '50-200', '200-1000', '1000-5000', '5000-10000', '10000-30000', '30000+']
    bins = [0, 50, 200, 1000, 10000, 50000, 100000, 300000, 500000]
    bin_labels = ['0-50', '50-200', '200-10E3', '10E3-10E4', '10E4-5*10E4', '5*10E4-10E5', '10E5-3*10E5', '3*10E5+']

    def get_histogram_counts(errors, bins):
        counts, _ = np.histogram(errors, bins=bins)
        # 处理大于最大bin的值
        counts[-1] += np.sum(errors >= bins[-1])  # 将超过500m的也归入最后一组
        return counts

    counts_aeconvlstm = get_histogram_counts(error_aeconvlstm, bins)
    counts_caekf = get_histogram_counts(error_caekf, bins)
    counts_immukf = get_histogram_counts(error_immukf, bins)

    bar_width = 0.25
    x_pos = np.arange(len(bin_labels))

    plt.figure(figsize=(14, 7))
    plt.bar(x_pos - bar_width, counts_aeconvlstm, width=bar_width, color='blue', label='Proposed', alpha=0.8)
    plt.bar(x_pos, counts_caekf, width=bar_width, color='green', label='CAEKF', alpha=0.8)
    plt.bar(x_pos + bar_width, counts_immukf, width=bar_width, color='red', label='IMMUKF', alpha=0.8)

    plt.xlabel("Error Range (m)", fontsize=12)
    plt.ylabel("Frequency", fontsize=12)
    # plt.title("Distribution of Position Errors", fontsize=14)
    plt.xticks(x_pos, bin_labels)
    plt.legend(fontsize=12)
    plt.grid(True, axis='y', alpha=0.5)
    plt.tight_layout()
    # plt.savefig("position_error_histogram.png", dpi=300)
    plt.show()
    # print("位置误差分布直方图已保存为 'position_error_histogram.png'")


# 图2：位置误差分布直方图（分组柱状图）
def plot_err_barchart_log(error_aeconvlstm, error_caekf, error_immukf):
    # 定义误差区间（例如每50米一个区间）
    # bins = [0, 50, 200, 1000, 5000, 10000, 30000, 50000]
    # bin_labels = ['0-50', '50-200', '200-1000', '1000-5000', '5000-10000', '10000-30000', '30000+']
    bins = [0, 50, 200, 1000, 10000, 50000, 100000, 300000, 500000]
    bin_labels = ['0-50', '50-200', '200-10E3', '10E3-10E4', '10E4-5*10E4', '5*10E4-10E5', '10E5-3*10E5', '3*10E5+']

    def get_histogram_counts(errors, bins):
        counts, _ = np.histogram(errors, bins=bins)
        # 处理大于最大bin的值
        counts[-1] += np.sum(errors >= bins[-1])  # 将超过500m的也归入最后一组
        return counts

    counts_aeconvlstm = get_histogram_counts(error_aeconvlstm, bins)
    counts_caekf = get_histogram_counts(error_caekf, bins)
    counts_immukf = get_histogram_counts(error_immukf, bins)

    eps = 1e-10
    counts_aeconvlstm_log = np.log10(counts_aeconvlstm + eps)
    counts_caekf_log = np.log10(counts_caekf + eps)
    counts_immukf_log = np.log10(counts_immukf + eps)

    bar_width = 0.25
    x_pos = np.arange(len(bin_labels))

    plt.figure(figsize=(14, 7))
    plt.bar(x_pos - bar_width, counts_aeconvlstm_log, width=bar_width, color='blue', label='Proposed', alpha=0.8)
    plt.bar(x_pos, counts_caekf_log, width=bar_width, color='green', label='CAEKF', alpha=0.8)
    plt.bar(x_pos + bar_width, counts_immukf_log, width=bar_width, color='red', label='IMMUKF', alpha=0.8)

    plt.xlabel("Error Range (m)", fontsize=12)
    plt.ylabel("Log10(Frequency) (log10(m))", fontsize=12)
    # plt.title("Distribution of Position Errors", fontsize=14)
    plt.xticks(x_pos, bin_labels)
    plt.legend(fontsize=12)
    plt.grid(True, axis='y', alpha=0.5)
    plt.tight_layout()
    # plt.savefig("position_error_histogram.png", dpi=300)
    plt.show()
    # print("位置误差分布直方图已保存为 'position_error_histogram.png'")


# ================================================== 主函数 ===============================================
def image_trace_err(path_csv_real, key_long_real, key_lati_real,
                    path_csv_aeconvlstm, key_long_aeconvlstm, key_lati_aeconvlstm,
                    path_csv_caekf, key_long_caekf, key_lati_caekf,
                    path_csv_immukf, key_long_immukf, key_lati_immukf):
    print("1. read .csv")
    # 读取真实值和3种算法的经纬度列表
    lon_real, lat_real = read_lonlat_from_csv(path_csv_real, key_long_real, key_lati_real)
    lon_aeconvlstm, lat_aeconvlstm = read_lonlat_from_csv(path_csv_aeconvlstm, key_long_aeconvlstm, key_lati_aeconvlstm)
    lon_caekf, lat_caekf = read_lonlat_from_csv(path_csv_caekf, key_long_caekf, key_lati_caekf)
    lon_immukf, lat_immukf = read_lonlat_from_csv(path_csv_immukf, key_long_immukf, key_lati_immukf)
    print("2. format")
    # 截取相同长度
    lon_real, lat_real = lon_real[1:], lat_real[1:]
    lon_aeconvlstm, lat_aeconvlstm = lon_aeconvlstm[1:], lat_aeconvlstm[1:]
    # 由于IMMUKF过于离谱, 截取开始部分数据
    # data_len = 5
    # lon_real, lat_real = lon_real[:data_len], lat_real[:data_len]
    # lon_aeconvlstm, lat_aeconvlstm = lon_aeconvlstm[:data_len], lat_aeconvlstm[:data_len]
    # lon_caekf, lat_caekf = lon_caekf[:data_len], lat_caekf[:data_len]
    # lon_immukf, lat_immukf = lon_immukf[:data_len], lat_immukf[:data_len]
    print("3. plot trace")
    # lon_* 和 lat_* 分别是真实值和3种轨迹预测方法(AE-CONV-LSTM, CAEKF, IMMUKF)的经纬度列表, 画出地图上三种算法和真实值的轨迹
    plot_trace(lon_real, lat_real, lon_aeconvlstm, lat_aeconvlstm, lon_caekf, lat_caekf, lon_immukf, lat_immukf)
    plot_trace_single(lon_real, lat_real, lon_aeconvlstm, lat_aeconvlstm)
    print("4. plot POS's error")
    # 计算与真实位置点的距离误差（单位：米）
    error_aeconvlstm = haversine_distance(lat_real, lon_real, lat_aeconvlstm, lon_aeconvlstm)
    error_caekf = haversine_distance(lat_real, lon_real, lat_caekf, lon_caekf)
    error_immukf = haversine_distance(lat_real, lon_real, lat_immukf, lon_immukf)
    # 时间序列索引（假设每个点为一个时间步）
    timesteps = np.arange(len(error_aeconvlstm))
    # 新增：打印每种算法误差的统计信息
    print("\n位置误差统计 (单位: 米):")
    print(f"{'算法':<15} {'最大值':<10} {'最小值':<10} {'平均值':<10}")
    print("-" * 50)
    print(f"{'AE-Conv-LSTM':<15} {np.max(error_aeconvlstm):<10.2f} "
          f"{np.min(error_aeconvlstm):<10.2f} {np.mean(error_aeconvlstm):<10.2f}")
    print(f"{'CAEKF':<15} {np.max(error_caekf):<10.2f} "
          f"{np.min(error_caekf):<10.2f} {np.mean(error_caekf):<10.2f}")
    print(f"{'IMMUKF':<15} {np.max(error_immukf):<10.2f} "
          f"{np.min(error_immukf):<10.2f} {np.mean(error_immukf):<10.2f}")
    # 画以下2张图:
    #  1. 位置误差时间序列对比图(折线图): 横轴为时间序列，纵轴为位置误差（米），三条曲线分别表示AE-Conv-LSTM、CAEKF、IMMUKF的误差随时间变化。
    plot_err_linechart(timesteps, error_aeconvlstm, error_caekf, error_immukf)
    plot_err_linechart_log(timesteps, error_aeconvlstm, error_caekf, error_immukf)
    #  2. 位置误差分布直方图(分组柱状图): 横轴为误差范围（0-50m, 50-100m, ...），纵轴为频次，三组柱状图对比三种算法的误差分布。
    plot_err_barchart(error_aeconvlstm, error_caekf, error_immukf)
    plot_err_barchart_log(error_aeconvlstm, error_caekf, error_immukf)


def image_trace_err_2(path_csv_real, path_csv_aeconvlstm, path_csv_caekf, path_csv_immukf):
    print("1. read .csv")
    # 读取真实值和3种算法的经纬度列表
    lon_real, lat_real = read_lonlat_from_csv(path_csv_real, "lon_smooth", "lat_smooth")
    lon_aeconvlstm, lat_aeconvlstm = read_lonlat_from_csv(path_csv_aeconvlstm, "lon_smooth", "lat_smooth")
    lon_caekf, lat_caekf = read_lonlat_from_csv(path_csv_caekf, "Longitude", "Latitude")
    lon_immukf, lat_immukf = read_lonlat_from_csv(path_csv_immukf, "Longitude", "Latitude")
    print("2. format")
    # 截取相同长度
    lon_real, lat_real = lon_real[1:], lat_real[1:]
    lon_aeconvlstm, lat_aeconvlstm = lon_aeconvlstm[1:], lat_aeconvlstm[1:]
    # 由于IMMUKF过于离谱, 截取开始部分数据
    # data_len = 5
    # lon_real, lat_real = lon_real[:data_len], lat_real[:data_len]
    # lon_aeconvlstm, lat_aeconvlstm = lon_aeconvlstm[:data_len], lat_aeconvlstm[:data_len]
    # lon_caekf, lat_caekf = lon_caekf[:data_len], lat_caekf[:data_len]
    # lon_immukf, lat_immukf = lon_immukf[:data_len], lat_immukf[:data_len]
    print("3. plot trace")
    # lon_* 和 lat_* 分别是真实值和3种轨迹预测方法(AE-CONV-LSTM, CAEKF, IMMUKF)的经纬度列表, 画出地图上三种算法和真实值的轨迹
    plot_trace(lon_real, lat_real, lon_aeconvlstm, lat_aeconvlstm, lon_caekf, lat_caekf, lon_immukf, lat_immukf)
    plot_trace_single(lon_real, lat_real, lon_aeconvlstm, lat_aeconvlstm)
    print("4. plot POS's error")
    # 计算与真实位置点的距离误差（单位：米）
    error_aeconvlstm = haversine_distance(lat_real, lon_real, lat_aeconvlstm, lon_aeconvlstm)
    error_caekf = haversine_distance(lat_real, lon_real, lat_caekf, lon_caekf)
    error_immukf = haversine_distance(lat_real, lon_real, lat_immukf, lon_immukf)
    # 时间序列索引（假设每个点为一个时间步）
    timesteps = np.arange(len(error_aeconvlstm))
    # 新增：打印每种算法误差的统计信息
    print("\n位置误差统计 (单位: 米):")
    print(f"{'算法':<15} {'最大值':<10} {'最小值':<10} {'平均值':<10}")
    print("-" * 50)
    print(f"{'AE-Conv-LSTM':<15} {np.max(error_aeconvlstm):<10.2f} "
          f"{np.min(error_aeconvlstm):<10.2f} {np.mean(error_aeconvlstm):<10.2f}")
    print(f"{'CAEKF':<15} {np.max(error_caekf):<10.2f} "
          f"{np.min(error_caekf):<10.2f} {np.mean(error_caekf):<10.2f}")
    print(f"{'IMMUKF':<15} {np.max(error_immukf):<10.2f} "
          f"{np.min(error_immukf):<10.2f} {np.mean(error_immukf):<10.2f}")
    # 画以下2张图:
    #  1. 位置误差时间序列对比图(折线图): 横轴为时间序列，纵轴为位置误差（米），三条曲线分别表示AE-Conv-LSTM、CAEKF、IMMUKF的误差随时间变化。
    plot_err_linechart(timesteps, error_aeconvlstm, error_caekf, error_immukf)
    plot_err_linechart_log(timesteps, error_aeconvlstm, error_caekf, error_immukf)
    #  2. 位置误差分布直方图(分组柱状图): 横轴为误差范围（0-50m, 50-100m, ...），纵轴为频次，三组柱状图对比三种算法的误差分布。
    plot_err_barchart(error_aeconvlstm, error_caekf, error_immukf)
    plot_err_barchart_log(error_aeconvlstm, error_caekf, error_immukf)


# 绘制单一轨迹(theta,phi)
def image_trace_propose_thph(path_y_out_test):
    # 读取CSV文件
    df = pd.read_csv(path_y_out_test)
    # 初始化列表存储转换后的坐标
    ori_x, ori_y = [], []
    out_x, out_y = [], []

    def spherical_to_cartesian(phi, theta):
        # x = 360 * np.sin(np.radians(theta)) * np.cos(np.radians(phi)) + 180
        # y = 360 * np.sin(np.radians(theta)) * np.sin(np.radians(phi)) + 180
        x = theta
        y = phi
        return x, y

    # 转换每一行的数据
    for index, row in df.iterrows():
        phi_ori, theta_ori = map(float, row['point_ori'].strip('()').split(','))
        phi_out, theta_out = map(float, row['point_out'].strip('()').split(','))
        x_ori, y_ori = spherical_to_cartesian(phi_ori, theta_ori)
        x_out, y_out = spherical_to_cartesian(phi_out, theta_out)
        ori_x.append(x_ori)
        ori_y.append(y_ori)
        out_x.append(x_out)
        out_y.append(y_out)
    # 创建图形和轴
    plt.figure(figsize=(12, 8))
    # 绘制折线图
    plt.plot(ori_x, ori_y, color='black', linestyle='--', linewidth=1.5, label='Real', zorder=5)
    plt.plot(out_x, out_y, color='blue', linewidth=2, label='Proposed')
    # 起点和终点标记
    plt.scatter(ori_x[0], ori_y[0], color='black', marker='o', s=100, zorder=6)
    plt.scatter(ori_x[-1], ori_y[-1], color='black', marker='s', s=100, zorder=6)
    plt.text(ori_x[0] + 0.0001, ori_y[0] + 0.0001, 'Start', fontsize=12, color='black')
    plt.text(ori_x[-1] + 0.0001, ori_y[-1] + 0.0001, 'End', fontsize=12, color='black')
    # 添加标题和标签
    # plt.title('Trajectories of point_ori, and point_out', fontsize=14)
    plt.xlabel('X', fontsize=12)
    plt.ylabel('Y', fontsize=12)
    # 添加图例
    plt.legend(fontsize=12)
    # 显示网格
    plt.grid(True, alpha=0.5)
    plt.tight_layout()
    # 显示图形
    plt.show()



if __name__=="__main__":
    # path_csv_dataset_test = "uav_dataset_thph_xy_lonlat_dataset_test.csv"
    # path_csv_caekf = "caekf_out_Xe_Measure4_2_coord_2.csv"
    # path_csv_immukf = "immukf_out_Xe_Measure4_2_coord_2.csv"
    # path_csv_aeconvlstm = "aeconvlstm_out_test_2_coord_2.csv"
    # key_long_real, key_lati_real = "lon_smooth", "lat_smooth"
    # key_long_aeconvlstm, key_lati_aeconvlstm = "lon_smooth", "lat_smooth"
    # key_long_caekf, key_lati_caekf = "Longitude", "Latitude"
    # key_long_immukf, key_lati_immukf = "Longitude", "Latitude"
    #
    path_csv_dataset_test = "uav_dataset_interpl_test_2025-10-21.csv"
    path_csv_caekf = "caekf_out_Xe_UAV_Measure_1_test_coord_2025-10-21.csv"
    path_csv_aeconvlstm = "aeconvlstm_out_UAV_Measure_1_test_coord_2025-10-21.csv"
    path_csv_immukf = "immukf_out_Xe_UAV_Measure_1_test_coord_2025-10-21.csv"
    key_long_real, key_lati_real = "long_smooth", "lati_smooth"
    key_long_aeconvlstm, key_lati_aeconvlstm = "aeconvlstm_long", "aeconvlstm_lati"
    key_long_caekf, key_lati_caekf = "Longitude", "Latitude"
    key_long_immukf, key_lati_immukf = "Longitude", "Latitude"
    #
    image_trace_err(path_csv_dataset_test, key_long_real, key_lati_real,
                    path_csv_aeconvlstm, key_long_aeconvlstm, key_lati_aeconvlstm,
                    path_csv_caekf, key_long_caekf, key_lati_caekf,
                    path_csv_immukf, key_long_immukf, key_lati_immukf)
    # image_trace_propose_thph("y_out_psll_point.csv")